#include "dyn_ode_dopri853.h"

DOPRI853::DOPRI853(MultiFunctor &func,
                   colvec _initCond,
                   double _start,
                   double _end,
                   double _initStepsize,
                   double _minStepsize,
                   double _maxStepsize,
                   double _absTol,
                   double _relTol,
                   int _maxIterations)
    : ODE(func,_initCond,_start,_end,_initStepsize,_minStepsize,_maxStepsize,_absTol,_relTol,_maxIterations)
{
}

colvec DOPRI853::next(colvec previousStep)
{
    while(1)
    {
        // We compute the k_i vectors.
        colvec k1 = initStepsize*integrand(previousStep);
        colvec k2 = initStepsize*integrand(previousStep+a21*k1);
        colvec k3 = initStepsize*integrand(previousStep+a31*k1+a32*k2);
        colvec k4 = initStepsize*integrand(previousStep+a41*k1+a43*k3);
        colvec k5 = initStepsize*integrand(previousStep+a51*k1+a53*k3+a54*k4);
        colvec k6 = initStepsize*integrand(previousStep+a61*k1+a64*k4+a65*k5);
        colvec k7 = initStepsize*integrand(previousStep+a71*k1+a74*k4+a75*k5+a76*k6);
        colvec k8 = initStepsize*integrand(previousStep+a81*k1+a84*k4+a85*k5+a86*k6+a87*k7);
        colvec k9 = initStepsize*integrand(previousStep+a91*k1+a94*k4+a95*k5+a96*k6+a97*k7+a98*k8);
        colvec k10 = initStepsize*integrand(previousStep+a101*k1+a104*k4+a105*k5+a106*k6+a107*k7+a108*k8+a109*k9);
        colvec k11 = initStepsize*integrand(previousStep+a111*k1+a114*k4+a115*k5+a116*k6+a117*k7+a118*k8+a119*k9+a1110*k10);
        colvec k12 = initStepsize*integrand(previousStep+a121*k1+a124*k4+a125*k5+a125*k6+a127*k7+a128*k8+a129*k9+a1210*k10+a1211*k11);

        // We compute the eighth-, fifth- and third-order increments.
        colvec eighthOrder = b1*k1+b6*k6+b7*k7+b8*k8+b9*k9+b10*k10+b11*k11+b12*k12;
        colvec fifthOrder = d1*k1+d6*k6+d7*k7+d8*k8+d9*k9+d10*k10+d11*k11+d12*k12;
        colvec thirdOrder = c1*k1+c6*k6+c7*k7+c8*k8+c9*k9+c10*k10+c11*k11+c12*k12;

        // We compute the difference of the methods, which gives
        // an error estimate delta. We take the user-supplied
        // variables absTol and relTol to form a scale variable.
        // We define an error estimate (W.H. Press et al, 2007. Numerical Recipes,
        // third edn, p.913).
        colvec delta5 = eighthOrder-fifthOrder;
        colvec delta3 = eighthOrder-thirdOrder;
        colvec scale = relTol*(previousStep+eighthOrder)+absTol;

        colvec normalizedSquaredError5 = pow(delta5/scale,2.0);
        colvec normalizedSquaredError3 = pow(delta3/scale,2.0);

        double error5 = sqrt((1./(normalizedSquaredError5.n_rows))*accu(normalizedSquaredError5));
        double error3 = sqrt((1./(normalizedSquaredError3.n_rows))*accu(normalizedSquaredError3));
        double error = (error5*error5)/(sqrt(pow(error3,2.0)+0.01*pow(error5,2.0)));

        // We compute the stepsize for the next
        // iteration if the steps succeeds and
        // the same step if it does not.
        double hold = initStepsize;
        double scaleN = pow(error,-alpha)*pow(errorOld,beta);

        // Controlling the rate of change of the stepsize.
        if (scaleN<0.2) {scaleN=0.2;}
        if (scaleN>10.0){scaleN=10.0;}

        // Changing the stepsize and verifying our bounds.
        initStepsize = 0.9*initStepsize*scaleN;
        if (initStepsize<minStepsize){initStepsize=minStepsize;}
        if (initStepsize>maxStepsize){initStepsize=maxStepsize;}

        // We recall the last error.
        errorOld = error;

        if (error <= 1.0)
        {
            oldStep = hold;
            return previousStep + fifthOrder;
        }
    }
}

int DOPRI853::integrate()
{
    cout << "Preparing DOPRI853 integration...";
    // We create a matrix having a single row,
    // but with enough columns to store all the data.
    int dimension = initCond.n_rows;
    mat tempResults = mat(1,dimension+1);

    // The first row contains the initial data.
    tempResults(0,0) = start;
    tempResults.submat(span(0,0),span(1,dimension)) = initCond.t();

    // We now compute the complete integral.
    // Since we do not know in advance on many
    // rows the results matrix will have, we use a
    // for loop to exit if we fail to do the integral.
    cout << "Integrating." << endl;
    for (int i=1;i<maxIterations;i++)
    {
        // We add a row to the results matrix.
        tempResults.resize(i+1,dimension+1);

        // We compute the values of the new row.
        tempResults.submat(span(i,i),span(1,dimension)) = next(tempResults.submat(span(i-1,i-1),span(1,dimension)).t()).t();
        tempResults(i,0) = tempResults(i-1,0)+getOldStep();

        if (tempResults(i,0) >= end)
        {
            i=maxIterations;
        }
    }

    results = tempResults;

    return 0;
}
